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Finding the best estimate of the process state from noisy data is the main 
problem in tracking systems, many efforts and researches have been done to 
remove this noise. More useful information about the target’s state can be 
extracted from observations by using a more appropriate model for the 
target’s motion or using additional sensors. In this paper, we will introduce 


two methods to improve the estimation of bearing-only target tracking 





problem in two dimensions (2D). The first method is by adding a third sensor 
Keywords: and making a good alignment of those sensors, and at the same time an 
AKF extended Kalman filter (EKF), unscented Kalman filter (UKF) and cubature 

Kalman filter (CKF) are implemented. The second method is by applying an 
ANKF adaptive nonlinear Kalman filter (ANKF) for two sensors to solve the 
ss problem of measurement variance uncertainty. 
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1. INTRODUCTION 

Different types of vehicles like cars, ships, planes perform different types of maneuvers from simple 
to complex. A single model does not represent all such maneuvers and particular multiple models are 
constructed to accurately describe and aid in predicting the trajectory according to the application [1]. 

Many specific dynamic models of target motion have been developed for maneuvering target 
tracking. The simplest model for a maneuvering target are the white-noise acceleration model [2]. Many 
target tracking algorithms are proposed based on so many different filters, such as alpha-beta filter, Gaussian 
sum filter. nonlinear Kalman filter and adaptive Kalman filter as in [3-4]. The Kalman filter is used for state 
estimation of some linear dynamic system [5-6], which can estimate the future state of the signal based on the 
signal statistical characteristic, the algorithm is formulated in two steps: prediction and updating. 
More common methods for dealing with a nonlinear model is to use the extended Kalman filter (EKF) [7]. 

Results of implementing non-linear Kalman filter for 2D tracking problems using two sensors 
showed that there are problems when the target is near the straight line which is between the two sensors [8]. 
The UKF algorithm is based on the Unscented Transform (UT) for computing the mean and covariance 
matrix of a variable, it is far easier to implement than the EKF [9]. For system model with a good linear and 
quadratic approximations the EKF works well but if the system is more nonlinear the UKF is often superior 
over EKF, the CKF algorithm is used in many applications like simultaneous localization and mapping [10], 
and in improving the tracking performance [11]. A Gaussian-sum cubature Kalman filter with improved 
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robustness demonstrated good accuracy and efficiency for the bearings-only tracking problem compared to 
the original algorithm of CKF [12]. In adaptive Kalman filter (AKF) the filter adjusts its knowledge about the 
mean and covariance matrix values according to the gap between the predicted estimations and the current 
measurements. A new methodology is proposed based on the use of weighted least squares methodology 
instead of traditional least square approach [13]. The estimation of parameter and state of nonlinear high- 
speed objects by adaptive unscented Kalman filter is proposed in [14]. The navigation system depends on 
sensors output, where the navigation output must be accurate and robust with erroneous measurements which 
caused from noise, that makes the noise process very important for sensors and control process in mobile 
robot [15]. Using a mobile robot for following sound sources in real time depending on an active virtual 
impedance algorithm for obstacle avoidance where the new algorithm changes the impedance dynamically in 
two cases: one distance sensor and two distance sensors, using more ultrasonic sensors for future research 
[16]. Implemented binary motion sensors for the problem of automated position estimation, using Passive 
Infrared (PIR) motion sensors with the suggested estimator, develop the detection system by different sensor 
models and arrangements is a future work [17]. A very important Kalman Filter use is to reduce the noise on 
the accelerometer and gyroscope sensors for quadrotor navigation system, where the bias and zero velocity 
compensations are able to improve the position estimation of the quadrotor by reducing the effects of drift 
due to integration [18]. 

The contributions of this study are the development in bearings-only target state estimation tracking 
problem by using many Kalman Filter algorithms (EKF, UKF, CKF) to reduce the noise on the sensor, 
improving the accuracy of estimation by adding a third sensor as hardware solution, which avoid the target 
movement on the straight line between two sensors, and change the alignment of those three sensors to have 
an accurate estimation. Another solution is software depending on [19] which is based on the innovation- 
based adaptive estimation ([AE) approach and the adaptive fading Kalman filter (AFKF) approach. 
This method was used to solve measurement variance uncertainty with two sensors in linear states. In this 
paper we will use the algorithms presented in [19] but we will change the flow chart of the AE, AFKF) 
algorithms to be suitable for nonlinear measurement as adaptive nonlinear Kalman filter (ANKF) for two 
sensors. Results will then be compared. 

The structure of this paper is as follows: Section two presents the bearing-only tracking model. 
Section three describes nonlinear Kalman filters (EKF, UKF, and CKF). Section four concerns proposed 
method using Adaptive Kalman filter implementation. In section five simulation results are compared 
(EKF, UKF, and CKF) in two sensors case, three sensors case and using proposed ANKF with two sensors 
case. Finaly conclusion will be presented in section six. 


2. BEARING-ONLY TRACKING MODEL 

Target motion and sensor measurement models in the two dimensional (2D) case are considered. 
In cartesian coordinates, the target state ex with position (X,, y,) and velocity (X,, y,) at time k is 
expressed as [8]: 


Cx = (Xe Ve Xe Ve)” (1) 


The dynamics of the target is modeled as a linear, discretized Wiener velocity model: 


10T O 
01 0T 
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Where T is the sampling time and o is the spectral density of the noise. The measurement model for sensor 


i is defined as: 


6, 
zZ, =he,,k)= ye +W, 


k 


(4) 





cin vi Bice " . 
Where @, = arctan(=" ~)+w, and (si Se ) is the position of sensor i and wi~N (0,03). 
Xx —S,. 


3. NONLINEAR KALMAN FILTER 
3.1. Extended Kalman Filter (EKF) 

The extended Kalmanis the nonlinear version of the Kalman filter which linearizes about an 
estimate of the current mean and covariance. First order extended Kalman filters are presented. The EKF 
model is [4], [8]: 


é, =f €,..k -D+V,, 
Z, =he,,k)+w, 


(5) 


Where the processing and measurement noise are white Gaussian with: 


Ve-1~N (0, Qx-1) 
on Ry) (6) 


The EKF algorithm: 
a. Prediction 


m, =f (m,_,,k -l) 
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b. Update 
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The m, and P, are the predicted mean and covariance of the state. 


Indonesian J Elec Eng & Comp Sci, Vol. 15, No. 1, July 2019 : 190 - 198 


Indonesian J Elec Eng & Comp Sci ISSN: 2502-4752 Oo 193 





3.2. Unscented Kalman Filter (UKF) 

In models with high non-linear predict and update functions (f 4) EKF gives particularly poor 
performance because the covariance is propagated through linearization of the underlying non-linear model. 
The unscented Kalman filter (UKF) is a sampling method uses a deterministic sampling technique known as 
the unscented transform (UT) to pick a minimal set of (2n+1) sample points called (sigma points) around the 
mean of the n-dimensional distribution of the covariance matrix, where computational efficiency is the 
advantage of the (UT). The UKF algorithm [4], [8], [9]: 

a) Sigma point 
The n-dimensional e, with mean é xj~ and covariance P.,, is approximated by 2n+1 weighted 


samples named sigma points selected using the following algorithm: 


Aen =€.,, Wy =K/(n+k) 


Kin =n +(J@ +K)P. Vee W, =1/2(n+k) 
Ait = Ey ix -(J@ +K)P. iy ) » W,,, =1/2n+K) 





(10) 


where X is a tuning parameter of the filter which is recommended to be chosen such that «+n =3, 
(las K)Peix ) , i=1,-+-,7, is the 7” row or column of the matrix square root of (n+«)P,,,, and W, is the 


I 


weight associated with the ag point. 
b) Prediction 
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3.3. Cubature Kalman Filter (CKF) 

The CKF is a Kalman-filter-based algorithm that uses the third-degree spherical-radial rule to 
generate cubature points with normalized weights to numerically approximate the multidimensional integrals 
involved in Bayesian filtering [8], [10], [11], [12]: 

a) Prediction 
- Calculate the cubature points from the intersection of n-dimension unit sphere and the Cartesian axes c;: 


Vic, i =l,...,n 
gi -| 


—Jnc,_, i=nt+l...,2n (14) 
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- Propagate the cubature points: 


Xian a = VP a aS FM ana (15) 
- Evaluate the cubature points with the dynamic model function: 

X i cana =f ® ican) (16) 
- Estimate the predicted state mean: 


1 p" 


Fae 2 eee 
Hoel (17) 
- Estimate the predicted error covariance: 


My 1K -l 


4 
P. Js x* x7 
k/k-1 2n y i,k -I/k -1 i,k -V/k-1 


i=l 
SH psig +O, (18) 
b) Update 


- Calculate the cubature points from the intersection of n-dimension unit sphere and the Cartesian. 
- Propagate the cubature points: 


Xi ina = VK asi FM (19) 


- Evaluate the cubature points with the measurement model function: 
Z - na =F Xi ana) (20) 


- Estimate the predicted measurement: 


. ae eee 
Zk ik-1 —Sz isk /k -1 
2n 7 (21) 
- Estimate the innovation covariance matrix: 
S Sty" aed 
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- Estimate the cross-covariance matrix: 
he ree 
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- Calculate the Kalman gain term and the smoothed state mean and covariance: 
-1 
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4. PROPOSED ALGORITHM - (ANKF) 

One of the KF problems occurs when the noise statistics Q and R are unknown, to solve this 
problem the adaptive Kalman filter is used. In this article we will use two algorithms which are presented 
in [19] while changing some steps according to nonlinear problem. The innovation-based adaptive estimation 
(IAE) approach and the adaptive fading Kalman filter (AFKF) approach for Linear Kalman filter are used 
in [19], where Figures 1 and 2, show those two algorithms. We changed the Block A by Block B and C 
which are the EKF algorithm for nonlinear problem. 


Ky = Pye He (HP, He + Ry) 
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Figure 1. Flow chart of the proposed ANKF1 method Figure 2. Flow chart of the proposeANKF2 method 


Where: 
a. Ap = diag[A,.Az ....Am] is a factor to option the new Py 
b. The algorithms of various adaptive fading are depending on A; values: 


A, <1 steady state processing 
A; >1 unstable filtring 
A; = 1 change to KF 


c. C,,: is calculated for deviation due to the changes of covariance and measurement noise. 





d. The optimum fading factors are: A; = (Ap) = max {1,10} ;f=1.2...m 
vk 
The covariance matrices can be defined as: 
. . = — tr (Cok) aS 
Single factor: A; = Ag) jj = tr(Cye) 4 =1.2...n 
: trCynij .; 
Multiple factors: A; = (Ag) jj; = ee i Sh 


tr (Cyn) jj” 


The adaptation measurement is: R;, = ApRx 
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5. SIMULATION RESULTS 
The EKF, UKF and CKF were applied to estimate the position of a target following two scenarios: 
-  Scenariol: two sensors. 
-  Scenario2: three sensors. 
The proposed method ANKF was applied for Scenario3 with two sensors. Table 1 shows the values 
of parameters for the three scenarios. 


Table 1. Value of Simulations Parameters 








Parameters Value 
2 
o 0.1 
G? (uncertainly) for adaptive 0.01 
0.05 
O7/, 
Target start state [0000] 
Covariance of the state on the initial time Po= diag [0.1, 0.1,10,10] 
T 0.01s 
Monte Carlo runs number 100 





5.1. Scenario 1: 

Sl(position)= [1-2]; S2(position)= [1-2]. Figure 3 shows the trajectory and the estimated 
trajectories. Figure 4 shows a zooming to the Figure 3 because results are so close. Table 2. shows the 
comparison between methods depending on Root Mean Square (RMS) error. 
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Figure 3. The trajectory of scenario 1 Figure 4. Zooming at the trajectory 


Table 2. Comparison between the Methods (Scenariol) 
Method RMS error 








EKF 0.2547 
UKF 0.2243 
CKF 0.2579 





5.2. Scenario 2: 

S1(position)= [1-2]; S2(position)= [1-2]; $3 (position)= [1-(2/tan(30)), 0] (we select the position of 
sensor3 to make equilateral triangle). 

Figure 5 shows the real trajectory and the estimated trajectories. Figure 6 shows zooming at Figure 5 
because the results are so close Table 3 shows the comparison between methods depending on RMS error. 


Table 3. Comparison between the Methods (Scenario2): 
Method RMS error 








EKF 0.13748 
UKF 0.06294 
CKF 0.13768 
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Figure 5. The trajectory of scenario 2 Figure 6. Zooming at the trajectory 
As expected, adding a third sensor decreased RMS for all algorithms and the UKF algorithm 
provides the best results. 


5.3. Scenario 3: 
In this scenario we used the adaptive Kalman filter with nonlinear measurement matrix from [4]: 


— (By\ _ tan" (y./Xx) 
hor) = ( eo) = Ps Mata?) (25) 


The two proposed adaptive Kalman filters algorithms are used, and compared with the most used 
algorithm EKF. Results are shown on Figures 7 and 8: 








X position y position 









—— Real 
«*** First Algorithm 
—— Second Algorithm 









—— Real 
«*** First Algorithm 
~~~ Second Algorithm 
77 EKF F 




















i 0 0.5 1 1.5 2 25 3 35 4 45 5 0 0.5 1 15 2 25 3 3.5 os 45 5 


Time (sec) Time (sec) 
Figure 7. The trajectory of scenario 3 Figure 8. Zooming at the trajectory 


We can see from Table 4 that the RMS error decreased by using adaptive Kalman filter with 
nonlinear measurement matrix comparing with Extended Kalman Filter (EKF). The ANKF2 algorithm 
provides the best result in estimation better than EKF and ANKFI with ratio of about 50%. 


Table 4. Comparison between the Methods (ANKF, EKF): 
Method RMS error 








EKF 0.5652 
ANKFI 0.5112 
ANKF2 0.2730 





6. CONCLUSION 

In this paper, we proposed nonlinear Kalman filter algorithms (EKF, UKF, and CKF) to improve 
and find the best estimate of the process state from noisy data, in bearings-only target state estimation 
tracking problem in three scenarios. In the first scenario we applied the past three algorithms with two 
sensors case, where the UKF gave the best RMS error by Monte Carlo 100 runs number. In the second 
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scenario we applied the algorithms and added a third sensor making equilateral triangle, the results showed 
that the RMS error with UKF is decreased with ratio about 50%. Surely, adding the third sensor in a good 
alignment will give better results but in some problems like mobile robot adding a third sensor is 
meaningless. This means the solution is software by developing algorithms. 

We used adaptive Kalman filter which is based on (I[AE) and (AFKF) to solve measurement 
variance uncertainty with two sensors in linear states, and changed the flow chart of the algorithm by 
applying an adaptive nonlinear Kalman filter (ANKF), the results of ANKFI and ANKF2 were better than 
EKF in view of RMS error, and the ANKF2 was the best. The following future work is to estimate the 
angular and angular rate for highly maneuvering targets in pitch and yaw with the nonlinear 
ANKF algorithms. 
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